#ifndef READ_DATA_H
#define READ_DATA_H
#include <iostream>
#include <sstream>
#include <fstream>
#include <vector>
#include <unistd.h>
#include <dirent.h>
#include <stdlib.h>
#include <stdio.h>
#include <map>
#include <queue>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <unordered_map>
#include <algorithm>
#include <memory>
#include <unordered_set>
#include <tuple>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "undistortion.h"

#include "common/map_builder.h"
#include "mapping/sliding_window.h"


using namespace std;
using namespace mapping;

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

struct Options {
    double odom_var_ = 0.05;
    double odom_span_ = 0.01;        // 里程计测量间隔
    double left_wheel_radius_ = 0.623479 / 2.0;   // 轮子半径
    double right_wheel_radius_ = 0.622806 / 2.0;   // 轮子半径
    double circle_pulse_ = 4096.0;  // 编码器每圈脉冲数
    double wheel_base_ = 1.52439;
    //double kl_ = 0.00047820240382508;
    //double kr_ = 0.00047768621928995;

    double kl_ = 1.0 / circle_pulse_ * M_PI * (2.0 * left_wheel_radius_);
    double kr_ = 1.0 / circle_pulse_ * M_PI * (2.0 * right_wheel_radius_);
};

template<typename T>
Eigen::Matrix<T,3,3> eulerToRotationMatrix(T roll, T pitch, T yaw) {
    // 转换为弧度
    roll *= M_PI/180.0f;
    pitch *= M_PI/180.0f;
    yaw *= M_PI/180.0f;
    
    // 计算三角函数值
    T cr = cos(roll); T sr = sin(roll);
    T cp = cos(pitch); T sp = sin(pitch);
    T cy = cos(yaw); T sy = sin(yaw);
    
    // 构建旋转矩阵 (Z-Y-X顺序)
    Eigen::Matrix<T,3,3> R;
    R << cy*cp,  cy*sp*sr - sy*cr,  cy*sp*cr + sy*sr,
         sy*cp,  sy*sp*sr + cy*cr,  sy*sp*cr - cy*sr,
         -sp,    cp*sr,            cp*cr;
    
    return R;
}
inline std::string& ltrim(std::string &s) {
    s.erase(s.begin(), std::find_if(s.begin(), s.end(), 
        [](int ch) { return !std::isspace(ch); }));
    return s;
}

// 去除右侧空白字符
inline std::string& rtrim(std::string &s) {
    s.erase(std::find_if(s.rbegin(), s.rend(), 
        [](int ch) { return !std::isspace(ch); }).base(), s.end());
    return s;
}

// 去除两侧空白字符
inline std::string& trim(std::string &s) {
    return ltrim(rtrim(s));
}

// 返回新字符串的版本（不修改原字符串）
inline std::string trim_copy(std::string s) {
    trim(s);
    return s;
}

enum class SensorDataType {
    KAIST = 0,
    NCLT
};

class OriginFeaturePoint
{
public:
    int x;
    int y;
    float score;
    float descriptor_data[128];
    OriginFeaturePoint(const int _x, const int _y, const float _score = 1.0);
};

class FeatureOutput
{
public:
    struct Timestamp
    {
        int32_t sec;
        int32_t nsec;
    };

    Timestamp timestamp;
    std::vector<OriginFeaturePoint> fp_vector;
    float global_feature[256];
};

class read_data {
public:
    read_data(const std::string& path, const SensorDataType& sensor_data_type = SensorDataType::KAIST);
    
    bool LoadSensorData(const std::string& encoder_file_path, std::unordered_map<std::string, std::string>* time_data_map);

    bool read_only_kaist_data(std::unordered_map<std::string, std::string>& time_encoder_map, 
                              std::unordered_map<std::string, std::string>& time_imu_map,
                              std::unordered_map<std::string, std::string>& time_gps_map, 
                              std::unordered_map<std::string, std::string>& time_vrs_gps_map,
                              std::unordered_map<std::string, std::string>& time_sick_middle_map,
                              std::unordered_map<std::string, std::string>& time_velodyne_left_map);

    std::unordered_map<std::string, std::string> read_only_nclt_data(const std::string& file_path);
    
    double get_origin_timestamp() {
        return origin_timestamp;
    }

    Eigen::Matrix3f get_ext_rot() {
        return ext_rot_vehicle2laser_;
    }
    Eigen::Vector3f get_ext_trans() {
        return ext_trans_vehicle2laser_;
    }

    Eigen::Matrix4f get_ext() {
        Eigen::Matrix4f Tvl = Eigen::Matrix4f::Identity();
        Tvl.block<3,3>(0,0) = ext_rot_vehicle2laser_;
        Tvl.block<3,1>(0,3) = ext_trans_vehicle2laser_;
        return Tvl;
    }

    bool LoadFrameData(std::vector<FeatureOutput>& results, std::vector<cv::KeyPoint>& undistorted_kps, const std::string& filename);
    
    void read_feature_txt(std::vector<feature::FeaturePoint>& feature_data, std::vector<cv::KeyPoint>& undistorted_kps, const Eigen::Matrix3d& intrinsic_matrix, const std::string& filename);

    Eigen::Matrix4d read_extrinsic();

    std::vector<feature::FeaturePoint> ConvertFeatureOutputToFeaturePoints(
            const std::vector<FeatureOutput>& results, const std::vector<OriginFeaturePoint>& filter_features, 
            const std::vector<cv::KeyPoint>& undistorted_kps, const Eigen::Matrix3d& intrinsic_matrix);
    
    std::vector<feature::FeaturePoint> ConvertTxtFeatureOutputToFeaturePoints(
            const std::vector<FeatureOutput>& results, 
            const std::vector<cv::KeyPoint>& undistorted_kps, const Eigen::Matrix3d& intrinsic_matrix);
    
    std::vector<FeatureOutput> LoadFrameTxtData(std::vector<FeatureOutput>& results, 
        std::vector<cv::KeyPoint>& undistorted_kps,
        const std::string& filename);

private:
    std::string path_;

    std::vector<std::string> split(std::string str, std::string pattern);

    double origin_timestamp = 0;

    Eigen::Matrix3f ext_rot_vehicle2laser_;
    Eigen::Vector3f ext_trans_vehicle2laser_;

    SensorDataType sensor_data_type_;

    Undistortion* undistortion_ptr_ = nullptr;

    Eigen::Matrix4d Tib_;

    Eigen::Matrix4d Tbc_;
    Eigen::Matrix4d Tbg_;
};


#endif


